from automatic import autoAgent
import cv2
import numpy as np
import matplotlib.pylab  as plt
from skimage import io
from control import controller
import time
from imageProcess import fineTuner

class vitureSelf():
    def __init__(self):
        time.sleep(5)
        print('vitureSelf init')

        self.controller = controller()
        self.fineTuner = fineTuner(self.controller)

        self.agent = autoAgent(self,self.controller)
        #self.loadbmp(0)
        self.main()
        #self.test2()
        return

        #for i in range(5):
        #   self.load(i)
    def test2(self):
        self.controller.writeCmd("85")
        self.controller.writeCmd("90")

        img = cv2.imread('c:/auto/a.bmp')
        if type(img) == None:
            print('img is None')
            return
        self.fineTuner.fineTuning(img)


    def hengxiangYidong(self,dist):
        print('横向移动 距离：' + str(dist))
        if dist < 0:
            self.controller.turnRight(90)
        else:
            self.controller.turnLeft(90)

        self.controller.goForword(np.abs(dist))

        if dist > 0:
            self.controller.turnRight(90)
        else:
            self.controller.turnLeft(90)

    def angleJiaozheng(self,angle):
        print('角度校正 目前角度：'+str(angle))
        angle = angle * 180 / np.pi
        if angle > 85 and angle < 95:
            return

        zhuanjiao = angle - 90

        self.controller.turnRight(zhuanjiao)



    def main(self):
        # img = self.load('p1.bmp')
        # self.agent.cheweiMean(img)
        # return


        print('测试启动')
        self.controller.setHorizontalCamera(85)
        self.controller.setVerticalCamera(20)
        i = 0
        result = None
        x = 0
        y = 0
        angle = 0
        while(i<12):
            i += 1
            img = self.loadRealPic()
            result = self.agent.getPic(img)
            if result == None:
                print('result is None')
                self.controller.turnRight(5)
            else:
                [x,y,angle] = result
                print('发现目标 目标坐标（%s,%s） 目标角度：%s'%(x,y,angle*180/np.pi))
                break
        if result == None:
            print('无法找到目标车位 进程终止')
            return

        self.enterChewei(x,y,angle,False)


        i = 0#车位位置精调
        result = None
        while(i<5):
            i += 1
            if self.cheweiJiaozhunYuan():
                break

        img = self.loadRealPic()
        result = self.agent.getPic(img)
        if result != None:
            [x,y,angle] = result
            self.enterChewei(x, y, angle, True,25)

            

        self.controller.setVerticalCamera(80)

        i = 0
        while i < 2:
            i += 1
            time.sleep(2)
            img = self.loadRealPic()
            [dy,dx,angle] = self.fineTuner.fineTuning(img)
            print([dy,dx,angle])

            if np.abs(angle - np.pi/2) > 0.2 and angle != 0:
                self.angleJiaozheng(angle)
            # elif np.abs(dx) > 0:
            #     self.hengxiangYidong(dx)
            elif dy > 3:
                self.controller.goForword(dy*0.5)
            else:
                break



        self.controller.setVerticalCamera(90)

        i = 0
        while i < 2:
            i += 1
            time.sleep(2)
            img = self.loadRealPic()
            [dy,dx,angle] = self.fineTuner.fineTuning(img)
            print([dy,dx,angle])

            if np.abs(angle - np.pi/2) > 0.2 and angle != 0:
                self.angleJiaozheng(angle)
            # elif np.abs(dx) > 0:
            #     self.hengxiangYidong(dx)
            elif dy > 3:
                self.controller.goForword(dy*0.5)
            else:
                break





    def enterChewei(self,x,y,angle,enter = False,goFowardMinus = 0):
        #在检测到车位的情况下向其前进
        print('enterChewei x:%s y:%s angle:%s enterBool:%s'%(x,y,angle,enter))
        if angle > np.pi / 2:
            zhuanxiangAngle = np.pi - angle
            zhuanxiangDist = y * np.cos(zhuanxiangAngle) + x * np.sin(zhuanxiangAngle)
            print("车位初次转向 转向角：%s 前进距离：%s"%(zhuanxiangAngle*180/np.pi,zhuanxiangDist))
            if zhuanxiangDist > 5:
                self.controller.turnRight(zhuanxiangAngle * 180 / np.pi)
                self.controller.goForword(zhuanxiangDist-5)
                self.controller.turnLeft(90)
            runDist = y * np.sin(zhuanxiangAngle) - x * np.cos(zhuanxiangAngle)
        elif angle < np.pi / 2:
            zhuanxiangAngle = angle
            zhuanxiangDist = y * np.cos(zhuanxiangAngle) - x * np.sin(zhuanxiangAngle)
            print("车位初次转向 转向角：%s 前进距离：%s" % (zhuanxiangAngle * 180 / np.pi, zhuanxiangDist))
            if zhuanxiangDist > 5:
                self.controller.turnLeft(zhuanxiangAngle * 180 / np.pi)
                self.controller.goForword(zhuanxiangDist*0.8)
                self.controller.turnRight(90)
            runDist = y * np.sin(zhuanxiangAngle) + x * np.cos(zhuanxiangAngle)

        if enter:
            self.cheweiJiaozhunYuan(0.1,3)
            self.controller.goForword(runDist - goFowardMinus)

    def cheweiJiaozhunYuan(self,yuzhi = 0.3,num = 1):
        #找不到车位时进行粗搜索 返回True代表无法继续
        for i in range(num):
            img = self.loadRealPic()
            result = self.agent.getPic(img)
            if result != None:
                print('发现车位处理，待添加，已经返回')
                return True
            else:
                [x_bias, y_bias] = self.agent.cheweiMean(img)
                if x_bias > yuzhi:
                    self.controller.turnRight(5 * x_bias / yuzhi)
                elif x_bias < -yuzhi:
                    self.controller.turnLeft(5 * x_bias / - yuzhi)
                else:
                    return True

    def load(self,i):
        print('load pic [%s]'%('img/%s'%(i)))
        img = cv2.imread('F:/bdnet/PythonAPI/auto/img/%s'%(i))
        return img


    def loadbmp(self,i):
        print('load pic [%s]'%('img/%s.bmp'%(i+1)))
        img = cv2.imread('img/%s.bmp'%(i+1))
        if type(img) == None:
            print('img is None')
            return
        self.agent.getPic(img,True)



    def loadRealPic(self):
        url='http://192.168.1.1:8080/?action=stream'
        cap=cv2.VideoCapture(url)
        ret=cap.isOpened()
        ret,img=cap.read()
        #cv2.imwrite('photo.bmp', img)
        cap.release()
        if type(img) == None:
            print('img is None')
            return None
        return img

print('startTest')
vitureSelf()
